#if 1 //只做回溯

#include <iostream>
#include <fstream>
#include <cstdlib>
#include <unistd.h>
#include "include/filtering_and_tracking1.h"
#include "include/data_format_conversion.h"
#include "include/UDPdriver.h"
#include "include/data_parser.h"
#include "include/public_struct.h"
#include <chrono>
#include "include/ClutterMap.h"
#include "include/DataSave.h"
#include <thread>
#include <mutex>
#include <queue>
#include <condition_variable>
#include <atomic>
#include <vector>

using namespace std;

//全局变量
queue<vector<uint8_t>> recv_queue;
mutex queue_mutex;
condition_variable queue_cv;

//接收数据线程函数
void udp_receive_thread(CUDPdriver *udp_recv_target, int max_len) {
    uint8_t buffer[1200];
    while (1) {
        int len = udp_recv_target->recv_data(buffer, max_len);
        if (len > 0) {
            vector<uint8_t> packet(buffer, buffer + len);
            {
                lock_guard<mutex> lock(queue_mutex);
                recv_queue.push(packet);
            }
            queue_cv.notify_one();
        }
    }
}

// 线程绑定CPU核心的函数
void set_thread_affinity(std::thread &t, int core_id) {
    // Linux系统实现
    cpu_set_t cpuset;
    CPU_ZERO(&cpuset);
    CPU_SET(core_id, &cpuset);  // 将线程绑定到core_id指定的核心

    pthread_t pthread = t.native_handle();
    int rc = pthread_setaffinity_np(pthread, sizeof(cpu_set_t), &cpuset);
    if (rc != 0) {
        // 绑定失败处理
        perror("线程绑定CPU核心失败");
    }

}

int main() {

    //接收数据
    const int local_port_target = 61001;      //接收目标接口 2808
    const int remote_port_display = 5566;    //上报显示端口

    CUDPdriver *udp_recv_target = new CUDPdriver();
    CUDPdriver *udp_sendto_display = new CUDPdriver();

    udp_recv_target->listen("172.16.1.70", local_port_target, 0); //223.255.255.28
    udp_sendto_display->connect("127.0.0.1", remote_port_display);

    printf("waiting for data\n");

    //数据解析
    MagicNumberInfo magic_number;
    DataParser *data_parser = new DataParser();

    //数据格式转化
    DataFormatConversion *data_convert = new DataFormatConversion();

    //数据处理
    DataProcessing *dataprocessing = new DataProcessing();

    //buffer
    const int max_recv_length = 1200;  // udp收数的长度
    uint8_t buffer[max_recv_length];
    memset(buffer, 0x0, max_recv_length);

    const int max_track_size = 12000;   // 1200可以15条，所以最多放150条航迹
    uint8_t track_buffer[max_track_size];
    memset(track_buffer, 0x0, max_track_size);

    static const uint32_t PACKET_PER_TARGRT_COUNTS = 24;   //单包发送点迹个数
    static const uint32_t FRAME_HEAD_SYN = 0x58445844;   //xd固定帧头
    static const uint16_t TARGET_DATA_TYPE = 0x0001;   //帧类型 点迹数据
    static const uint16_t VIDEO_DATA_TYPE = 0x0000;   //帧类型 视频数据

    FrameHeadTarget *p_frame_head;
    TargetData *p_target_data;

    int target_count = 0;   //每帧的点迹数
    int pac_count = 0;      //每帧的包数
    int frame_num = 0;      //帧编号 从0开始
    int frame_now = 0;      //当前帧编号
    int pac_num = 0;
    int beam_azi_id = 0; //方位波束编号
    int beam_azi_now = 0;
    int beam_pitch_id = 0; //俯仰波束编号

    vector<PointIn> in_dot;  //udp解析出的数据
    vector<PointIn> in_dot_rx;  //存满一帧点迹

    const int doppler_num_half = 32;
    int udpRecv_dataSize = 0;
    in_dot.clear();

    FILE *fp1 = fopen("../data/20250919_202700.dat", "rb+"); // 读取数据
    FILE *fp2 = fopen("../data/in_dot.txt", "w+"); // 写入原始点迹数据
    FILE *fp3 = fopen("../data/input.txt", "w+"); // 写入点迹数据
    FILE *fp4 = fopen("../data/frame.txt", "w+"); // 写入点迹数据


    bool isStart = false; //是否开始接收数据
    bool flagnext = false; //下一帧标志
    bool azi_flag = false; //方位标志
    bool circle_flag = false; //圈数标志
    int circle = 0; //圈数
    int circle_out = 0; //输出圈数
    int circle_dot = 0;
    auto start = chrono::high_resolution_clock::now();
    auto end = chrono::high_resolution_clock::now();

    vector<PointIn> in_dot_azi;  //一个方向的所有点迹
    vector<PointIn> in_dot_azi_con;  //一个方向的所有点迹
    vector<PointIn> in_dot_azi_pre;  //一个方向的所有点迹
    vector<PointIn> in_dot_out;  //一个方向的所有点迹
    vector<PointIn> in_dot_out_clutter;  //一个方向的所有点迹
    vector<PointInfo> input;  //滤波跟踪的输入
    vector<TrackOut> output;  // 滤波跟踪的输出
    bool readFileFlag = true; //是否读取文件
    readFileFlag = false;
    thread recv_thread;
    DataSave dataSave;

    if (!readFileFlag) {
        recv_thread = thread(udp_receive_thread, udp_recv_target, max_recv_length);//创建收数线程
        set_thread_affinity(recv_thread, 1); // 将接收线程绑定到CPU核心1
        dataSave.startSave();//开启存数
    }

    while (1) {
        if (readFileFlag) {
            udpRecv_dataSize = fread(buffer, 1, 1200, fp1);
        } else {
            std::vector<uint8_t> data;

            {
                std::unique_lock<std::mutex> lock(queue_mutex);
                queue_cv.wait(lock, [] {
                    return !recv_queue.empty();
                });
//                cout<<"recv_queue_size: "<< recv_queue.size()<<endl;
                data = recv_queue.front();
                recv_queue.pop();
            }
            if (!data.empty()) {
                memcpy(buffer, data.data(), data.size());
                udpRecv_dataSize = data.size();
                dataSave.push(buffer);
            }
        }

        if (udpRecv_dataSize) {
            p_frame_head = reinterpret_cast<FrameHeadTarget *>(buffer);
            if (true || p_frame_head->frame_head_syn == FRAME_HEAD_SYN) {
                if (true | p_frame_head->frame_type == TARGET_DATA_TYPE) {

                    target_count = p_frame_head->trace_counts;    //当前帧的点迹个数
                    pac_count = p_frame_head->package_count;    //最大包数
                    pac_num = p_frame_head->package_number;    //当前包编号
                    frame_num = p_frame_head->frame_number; //当前帧编号
                    beam_azi_id = p_frame_head->beam_azi_id; //当前帧的方位波束编号
                    beam_pitch_id = p_frame_head->beam_pitch_id; //当前帧的俯仰波束编号

                    fprintf(fp4, "target_count = %d pac_count = %d pac_num = %d frame_num = %d beam_azi_id = %d "
                                 "beam_pitch_id = %d\n", target_count, pac_count, pac_num, frame_num, beam_azi_id,
                            beam_pitch_id);
                    fflush(fp4);

                }
                if (beam_azi_id == 0 && beam_pitch_id == 0) isStart = true;

                if (!isStart) continue;
                if (frame_now == 0) frame_now = frame_num;

                //存储每帧数据
                if (frame_num != frame_now) {
                    if (frame_num != frame_now + 1)
                        cout << "帧号不连续：frame_num = " << frame_num << "frame_now = " << frame_now << endl;
                    int N = in_dot.size();
                    circle_dot+=N;
                    if (beam_azi_id != beam_azi_now) {
                            azi_flag = true;
                            if (beam_azi_id == 0) {
                                circle++;
                                circle_flag = true;
                            }
                        beam_azi_now = beam_azi_id;
                    }
//                    printf( "frame_num = %d, target_count = %d\n", frame_num, N);
                    //存储原始点迹
                    if (N > 0)
                        fprintf(fp2, "frame_now = %d, target_count = %d\n", frame_now, N);
                    for (int i = 0; i < N; i++) {
                        in_dot_rx.push_back(in_dot[i]);
                        fprintf(fp2,
                                "dis = %.2f, dop = %d, azi = %.2f, pitch = %.2f, snr = %d, amp = %d,mean_noise = %d frame = %d, beam_id_azi = %hu, beam_id_pitch = %hu,shanqu=%d\n",
                                in_dot[i].Range, in_dot[i].Doppler, in_dot[i].Azimuth, in_dot[i].pitch,
                                in_dot[i].SNR, in_dot[i].Amplitude, in_dot[i].mean_noise, in_dot[i].frame,
                                in_dot[i].beam_id_azi,
                                in_dot[i].beam_id_pitch,
                                in_dot[i].shanqu);
                    }
                    fflush(fp2);
                    in_dot.clear();
                    frame_now = frame_num;
                    flagnext = true;
                }
                //解析数据
                int k = pac_num;
                if (k < pac_count) {
                    p_target_data = reinterpret_cast<TargetData *>(buffer + sizeof(FrameHeadTarget) * (k + 1) +
                                                                   sizeof(TargetData) * k);
                    for (int j = 0; j < target_count; j++) {
                        PointIn tmp_dot;
                        tmp_dot.Range = p_target_data[j].distance / 10.0;
                        tmp_dot.Azimuth = p_target_data[j].azimuth / 100.0;
                        tmp_dot.Amplitude = p_target_data[j].amplitude / 10.0;
                        tmp_dot.pitch = p_target_data[j].pitch / 100.0;//协议是否需要/10
                        tmp_dot.mean_noise = p_target_data[j].mean_noise/10;
                        tmp_dot.frame = frame_num;
                        tmp_dot.SNR = p_target_data[j].SNR / 10.0;
                        tmp_dot.beam_id_azi = beam_azi_id;
                        tmp_dot.beam_id_pitch = beam_pitch_id;
                        tmp_dot.shanqu = p_target_data[j].shanqu;
                        //多普勒移位
                        tmp_dot.Doppler = p_target_data[j].doppler;
                        if (tmp_dot.Doppler < doppler_num_half) tmp_dot.Doppler += doppler_num_half;
                        else tmp_dot.Doppler -= doppler_num_half;

                        //筛选条件
                        if (!isStart) continue;
                        if (tmp_dot.Range < 200 || tmp_dot.Range > 5000
//                                                                                    || tmp_dot.Doppler <34
//                                                                                    || tmp_dot.Doppler >42
                            //                            || tmp_dot.Doppler != 34
                            || (tmp_dot.Range < 2100 && tmp_dot.SNR < 10)
                            || (tmp_dot.Range < 1000 && tmp_dot.SNR < 15)
                            || (tmp_dot.Range < 500 && tmp_dot.SNR < 30)
//                            || tmp_dot.Azimuth < -42 || tmp_dot.Azimuth > -39.5
                                )
                            continue;
                        in_dot.push_back(tmp_dot);
                    }
                }
            }
        } else {
//            fseek(fp1,0,SEEK_SET);
//            cout<<"again"<<endl;
            std::cout << "Read Over" << endl;
            break;
        }

        //凝聚
        if (flagnext) {

            //储存一个方位不同俯仰
            in_dot_azi.insert(in_dot_azi.end(), in_dot_rx.begin(), in_dot_rx.end());
            in_dot_rx.clear();

            //同一个方向的凝聚
            if (azi_flag) {
                data_convert->data_select(in_dot_azi, in_dot_azi_con);
                in_dot_azi.clear();
            }

            //相邻方位的凝聚
            if (azi_flag) {
                data_convert->data_con(in_dot_azi_pre, in_dot_azi_con, in_dot_out, circle_flag);
                azi_flag = false;
            }

        }

        //数据处理
        if (circle_flag) {
            //打印输入点迹
            fprintf(fp3, "圈数:%d 点数：%d\n", circle, in_dot_out.size());
            for (int a = 0; a < in_dot_out.size(); a++) {
                fprintf(fp3,
                        "dis = %.2f, dop = %d, azi = %.2f, pitch = %.2f, snr = %d, frame = %d, beam_id_azi = %hu, beam_id_pitch = %hu\n",
                        in_dot_out[a].Range, in_dot_out[a].Doppler, in_dot_out[a].Azimuth,
                        in_dot_out[a].pitch,
                        in_dot_out[a].SNR, in_dot_out[a].frame, in_dot_out[a].beam_id_azi,
                        in_dot_out[a].beam_id_pitch);
            }
            fflush(fp3);

            //格式转化
            data_convert->conversion(in_dot_out, input);
            in_dot_out.clear();
            output.clear();
            circle_out++;
            dataprocessing->processing_per_circle(input, output, circle_out);

            //打包数据
            data_parser->pack_track_output(output, track_buffer, max_track_size);

            //发送数据到上位机
            memcpy(&magic_number, track_buffer, sizeof(MagicNumberInfo));
            for (int i = 0; i < magic_number.UDP_cnt; i++) {
                udp_sendto_display->send_data(
                        track_buffer + i * magic_number.FrameLength,
                        magic_number.FrameLength);
            }

            //打印航迹信息
            printf("圈数：%d 点数：%d 点数2：%d\n", circle_out,input.size(),circle_dot);
            for (std::size_t i = 0; i < output.size(); i++) {
                printf(
                        "{id, num, range, azimuth, pitch, velocity, SNR, deletion_count,dotbianhao="
                        "{%d, %d, %.2f, %.2f, %.2f, %.2f, %.2f, %d, %d,}\n",
                        output.at(i).Track_ID,
                        output.size(),
                        output.at(i).Track_Range,
                        output.at(i).Track_Azimuth,
                        output.at(i).pitchAngle,
                        output.at(i).Track_Velocity,
                        output.at(i).SNR,
                        output.at(i).deletion_count,
                        output.at(i).dotbianhao

                );
            }
            input.clear();
            circle_flag = false;
            circle_dot = 0;
            //连接显控时打开看航迹延时
            if (readFileFlag)
                usleep(100000);
            start = end;
            end = chrono::high_resolution_clock::now();
            auto ms = chrono::duration_cast<chrono::milliseconds>(end - start).count();
            cout << " 运算时间：" << ms << "ms" << endl;

        }
    }
    fclose(fp1);
    fclose(fp2);
    fclose(fp3);
    fclose(fp4);
    exit(0);
}

#endif